package com.grid.camera.pelco;

import lombok.extern.slf4j.Slf4j;

@Slf4j
/** Pelco D 协议实现类 Pelco D 是一种常用的云台控制协议，用于控制PTZ摄像机 */
public class PelcoDProtocol {
    // Pelco D 命令常量
    public static final byte COMMAND_NONE = 0x00;

    // 上行
    public static final byte COMMAND_UP = 0x09;
    // 下行
    public static final byte COMMAND_DOWN = 0x09;
    // 操作停止
    public static final byte COMMAND_STOP = 0x0B;

    // 预设位命令
    public static final byte COMMAND_SET_PRESET = 0x03;
    public static final byte COMMAND_CLEAR_PRESET = 0x05;
    // 调用预置位
    public static final byte COMMAND_GOTO_PRESET = 0x07;
    // 读取预设位
    public static final byte COMMAND_READ_PRESET = 0x47;

    // 协议固定值
    private static final byte SYNC_BYTE = (byte) 0xFF;

    /**
     * 生成清除预设位命令
     *
     * @param address 设备地址
     * @param presetNumber 预设位编号 (1-255)
     * @return Pelco D协议数据包
     */
    public static byte[] clearPreset(int address, int presetNumber) {
        log.info("清除预设位: {}", "clearPreset");
        return generatePacket(
                address, COMMAND_NONE, COMMAND_CLEAR_PRESET, (byte) 0, (byte) presetNumber);
    }

    /**
     * 生成调用预设位命令
     *
     * @param address 设备地址
     * @param presetNumber 预设位编号 (1-255)
     * @return Pelco D协议数据包
     */
    public static byte[] gotoPreset(int address, int presetNumber) {
        log.info("调用预设位: {}", "gotoPreset");
        return generatePacket(
                address, COMMAND_NONE, COMMAND_GOTO_PRESET, (byte) 0, (byte) presetNumber);
    }

    /**
     * 生成云台下移命令
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] moveDown(int address) {
        log.info("云台下移: {}", "moveDown");
        return generatePacket(address, COMMAND_NONE, COMMAND_DOWN, (byte) 0, (byte) 5);
    }

    /**
     * 生成云台下移停止命令
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] moveDownStop(int address) {
        log.info("云台下移停止: {}", "moveDownStop");
        return generatePacket(address, COMMAND_NONE, COMMAND_STOP, (byte) 0, (byte) 5);
    }

    /**
     * 生成云台上移命令
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] moveUp(int address) {

        log.info("云台上移: {}", "moveUp");
        return generatePacket(address, COMMAND_NONE, COMMAND_UP, (byte) 0, (byte) 6);
    }

    /**
     * 停止云台上移
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] moveUpStop(int address) {
        log.info("停止云台上移: {}", "moveUpStop");
        return generatePacket(address, COMMAND_NONE, COMMAND_STOP, (byte) 0, (byte) 6);
    }

    /**
     * 读取硬件版本
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] readHardwareVersion(int address) {
        log.info("读取硬件版本: {}", "readHardwareVersion");
        return generatePacket(address, (byte) 01, (byte) 01, (byte) 0, (byte) 1);
    }

    /**
     * 读取预设位
     *
     * @param address 设备地址
     * @param presetNumber 预设位编号 (1-255)
     * @return Pelco D协议数据包
     */
    public static byte[] readPreset(int address, int presetNumber) {
        log.info("读取预设位: {}", "readPreset");
        return generatePacket(
                address, COMMAND_NONE, COMMAND_READ_PRESET, (byte) 0, (byte) presetNumber);
    }

    /**
     * 读取下侧避障
     *
     * @param address 设备地址
     */
    public static byte[] readSideObstacle(int address) {
        log.info("读取下侧避障: {}", "readSideObstacle");
        return generatePacket(address, (byte) 0x00, (byte) 0x1C, (byte) 0x00, (byte) 0x01);
    }

    /**
     * 读取软件版本
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] readSoftwareVersion(int address) {
        log.info("读取软件版本: {}", "readSoftwareVersion");
        return generatePacket(address, (byte) 01, (byte) 02, (byte) 0, (byte) 1);
    }

    /**
     * 读取云台状态信息
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] readStatus(int address) {
        log.info("读取云台状态信息: {}", "readStatus");
        return generatePacket(address, (byte) 0x01, (byte) 0x03, (byte) 0x00, (byte) 0xff);
    }

    /**
     * 读取垂直方向驱动状态
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] readVerticalDriveStatus(int address) {
        log.info("读取垂直方向驱动状态: {}", "readVerticalDriveStatus");
        return generatePacket(address, (byte) 0x0f, (byte) 0xff, (byte) 0x03, (byte) 0x00);
    }

    /**
     * 读取云台垂直编码器状态
     *
     * @param address
     * @return
     */
    public static byte[] readVerticalEncoderStatus(int address) {
        log.info("读取云台垂直编码器状态: {}", "readVerticalEncoderStatus");
        return generatePacket(address, (byte) 0x0f, (byte) 0xff, (byte) 0x05, (byte) 0x00);
    }

    /**
     * 读取云台垂直速度
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] readVerticalSpeed(int address) {
        log.info("读取云台垂直速度: {}", "readVerticalSpeed");
        return generatePacket(address, (byte) 0x0e, (byte) 0x06, (byte) 0x00, (byte) 0x01);
    }

    /**
     * 复位系统
     *
     * @param address
     * @return
     */
    public static byte[] resetSystem(int address) {
        log.info("复位系统: {}", "resetSystem");
        return generatePacket(address, (byte) 0x0f, (byte) 0x01, (byte) 0, (byte) 1);
    }

    /**
     * 设置垂直轨道最大位置
     *
     * @param address
     * @return
     */
    public static byte[] setMaxPosition(int address) {
        log.info("设置垂直轨道最大位置: {}", "setMaxPosition");
        return generatePacket(address, COMMAND_NONE, (byte) 0x0d, (byte) 0, (byte) 2);
    }

    /**
     * 设置预设位命令
     *
     * @param address 设备地址
     * @param presetNumber 预设位编号 (1-255)
     * @return Pelco D协议数据包
     */
    public static byte[] setPreset(int address, int presetNumber) {
        log.info("设置预设位命令: {}", "setPreset");
        return generatePacket(
                address, COMMAND_NONE, COMMAND_SET_PRESET, (byte) 0, (byte) presetNumber);
    }

    /**
     * 设置下侧避障
     *
     * @param address 设备地址
     * @param distance 障碍物距离
     */
    public static byte[] setSideObstacle(int address, int distance) {
        log.info("下侧避障: {}", "setSideObstacle");
        byte highByte = (byte) (distance >> 8);
        byte lowByte = (byte) (distance & 0xFF);
        return generatePacket(address, (byte) 0x00, (byte) 0x0C, highByte, lowByte);
    }

    /**
     * 设置云台垂直速度
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] setVerticalSpeed(int address, int speed) {
        log.info("设置云台垂直速度: {}", "setVerticalSpeed");
        byte highByte = (byte) (speed >> 8);
        byte lowByte = (byte) (speed & 0xFF);
        return generatePacket(address, (byte) 0x0e, (byte) 0x04, highByte, lowByte);
    }

    /**
     * 停止命令
     *
     * @param address 设备地址
     * @return Pelco D协议数据包
     */
    public static byte[] stopMovement(int address) {
        log.info("停止命令: {}", "stopMovement");
        return generatePacket(address, COMMAND_NONE, COMMAND_STOP, (byte) 0, (byte) 15);
    }

    /**
     * 生成Pelco D协议数据包
     *
     * @param address 设备地址 (1-255)
     * @param command1 命令1
     * @param command2 命令2
     * @param data1 数据1
     * @param data2 数据2
     * @return 完整的Pelco D协议数据包
     */
    private static byte[] generatePacket(
            int address, byte command1, byte command2, byte data1, byte data2) {
        byte[] packet = new byte[7];

        packet[0] = SYNC_BYTE; // 同步字节
        packet[1] = (byte) address; // 地址
        packet[2] = command1; // 命令1
        packet[3] = command2; // 命令2
        packet[4] = data1; // 数据1
        packet[5] = data2; // 数据2

        // 计算校验和: 地址 + 命令1 + 命令2 + 数据1 + 数据2 的和取低8位
        packet[6] = calculateChecksum(packet);

        log.info("发送数据包: {}", PelcoDUtils.bytesToHex(packet));

        return packet;
    }

    /**
     * 计算校验和
     *
     * @param packet 数据包
     * @return 校验和 (字节2-6加和的16进制值)
     */
    private static byte calculateChecksum(byte[] packet) {
        int sum = 0;
        // 计算字节2-6的加和（索引1-5）
        for (int i = 1; i < 6; i++) {
            sum += packet[i] & 0xFF;
        }
        // 返回16进制值（取低8位）
        return (byte) (sum & 0xFF);
        //        return (byte) (sum);
    }

    /**
     * 验证速度值在有效范围内 (0-63)
     *
     * @param speed 速度值
     * @return 有效的速度值
     */
    private static byte validateSpeed(int speed) {
        if (speed < 0) {
            return 0;
        } else if (speed > 63) {
            return 63;
        }
        return (byte) speed;
    }
}
